#include "Cam.h"
#include "LdPcl.h"
#include "odometry.h"
using namespace std;
int main()
{
    std::string topic;
    std::string ips;
    std::vector<float> yaw;
    std::vector<float> Tx, Ty, Tz;
    int pointsNum;
    int cloudsNum;
    int img_W, img_H;
    bool dynamic;
    int KsearchNum;
    // cv::Mat cvRoT(4, 4, CV_32F);
    // cv::Mat cvCamIntrix(3, 3, CV_32F);
    // Eigen::Matrix3f eiCamIntrix;
    // Eigen::Matrix4f eiRoT;
    Eigen::Matrix3f camIntrix = Eigen::Matrix3f::Identity();
    Eigen::Matrix4f CL_RoT = Eigen::Matrix4f::Identity();
    std::string cfgFile = "../YmlFiles/cfg.yml";
    PCLF pclFunc(cfgFile);
    // pclFunc.PCLfunctionsCfg(cfg_file);
    cv::FileStorage LoadMycfg_main("../YmlFiles/cfg.yml", cv::FileStorage::READ);
    LoadMycfg_main["PointsNum"] >> pointsNum;   //单帧点数
    LoadMycfg_main["CloudsNum"] >> cloudsNum;   //合成帧数
    LoadMycfg_main["Dynamic"] >> dynamic;       //是否启动动态建图
    LoadMycfg_main["Width"] >> img_W;           //深度图Width，大华为1280
    LoadMycfg_main["Height"] >> img_H;          //深度图Height，大华为1024
    LoadMycfg_main["Topic"] >> topic;           //LCM通信的主题，为"POINT"
    LoadMycfg_main["Ips"] >> ips;               //LCM通信IP
    LoadMycfg_main["KsearchNum"] >> KsearchNum; //法线估计时，K-means搜索半径内的点数
    // LoadMycfg_main["cvRoT"] >> cvRoT;
    // LoadMycfg_main["cvCamIntrix"] >> cvCamIntrix;
    LoadMycfg_main.release();
    // cv::cv2eigen(cvRoT, eiRoT);
    // cv::cv2eigen(cvCamIntrix, eiCamIntrix);
    // std::cout << "eiRoT is: " << eiRoT << std::endl;
    // std::cout << "eiCamIntrix is: " << eiCamIntrix << std::endl;
    //测试LCM的程序段
    // while (true)
    // {
    //     Comlcm testLCM(topic, ips);
    //     std::vector<CorLag> Coords;
    //     for (size_t i = 0; i < 5; i++)
    //     {
    //         CorLag tempCoor;
    //         tempCoor.id = i;
    //         tempCoor.center.x() = i + 100;
    //         tempCoor.center.y() = i + 200;
    //         tempCoor.Lcolor = false;
    //         tempCoor.radius = 200;
    //         Coords.push_back(tempCoor);
    //     }
    //     testLCM.Publish(Coords);
    // }
    //<--测试LCM的程序段

    //导入相机内参和雷达相机旋转矩阵
    YAML::Node MatrixFile = YAML::LoadFile(cfgFile);
    const std::vector<float> IntrixVec = MatrixFile["CamIntrix"].as<std::vector<float>>();
    const std::vector<float> RoTVec = MatrixFile["RoT"].as<std::vector<float>>();
    Eigen::Matrix<float, 3, 3, Eigen::RowMajor> tCamIntrix(IntrixVec.data());
    Eigen::Matrix<float, 4, 4, Eigen::RowMajor> RoT(RoTVec.data());
    CL_RoT.swap(RoT);
    camIntrix.swap(tCamIntrix);
    std::cout << "YAML RoT is: " << CL_RoT << std::endl;
    std::cout << "YAML camIntrix is: " << camIntrix << std::endl;

    cv::namedWindow("OriginImg", cv::WINDOW_FREERATIO);
    cv::namedWindow("DepthImg", cv::WINDOW_FREERATIO);
    cv::namedWindow("copy", cv::WINDOW_FREERATIO); //用于显示yolo识别成功的图片

    //加载配置文件
    std::string names_file = "../include/darknet/files/obj.txt";
    std::string yolo_cfg_file = "../include/darknet/files/yolov4-tiny-obj.cfg";
    std::string weight_file = "../include/darknet/files/yolov4-tiny-obj_last.weights";

    //创建LCM对象
    Comlcm LCMobj(topic, ips);
    std::vector<CorLag> coordinateLags; //存储所有识别到的Lagori的信息

    //启动雷达
    std::vector<std::string> BroadCode;
    BroadCode.push_back("0TFDFG700601861");
    LdsLidar& mylidar = LdsLidar::GetInstance();
    int init = mylidar.InitLdsLidar(BroadCode, pointsNum, cloudsNum, dynamic); //Dynamic为是否启动动态建图，false为不启动

    // //开启串口
    // Gyodometry &Odometry = Gyodometry::GetInstance();
    // std::thread OdoThread(&Gyodometry::DataCL, &Odometry);

    //开启相机
    Cam myCam(names_file, yolo_cfg_file, weight_file);
    std::thread CamThread(&Cam::grab, &myCam);

    //缓冲时间，给予相机和雷达进行初始采样
    sleep(1);

    std::vector<std::vector<cv::Point>> Lpoints;
    std::vector<int> Tags;
    PcloudI Lcloud(new CloudI());
    double stime, etime;
    struct timeval timer;
    gettimeofday(&timer, NULL);
    stime = timer.tv_sec * 1e3 + timer.tv_usec * 1e-3;
    while (true)
    {
        PcloudI OriginCloud(new CloudI());
        cv::Mat DepthImg = cv::Mat::zeros(img_H, img_W, CV_16UC1);
        cv::Mat OriginImg;
        cv::Mat copyOrigin;

        //动态建立点云图
        int count = 1;

        if (dynamic == true) // 动态建图
        {                    // 验证动态抓图用
            std::vector<CloudI> clouds;
            std::vector<Gydata> Gys;
            clouds.resize(cloudsNum);
            Gys.resize(cloudsNum);
            std::thread Thread_saveImg(&Cam::getImg, &myCam); //存图
            Thread_saveImg.detach();
            mylidar.getall(clouds, Gys); //获取点云图和深度图

            myCam.getOriginImg(OriginImg);
            std::thread Thread_detect(&Cam::detectLag, &myCam, OriginImg); //创建yolo识别线程
            Thread_detect.detach();
            PcloudI dest(new CloudI());
            *dest = clouds[cloudsNum - 1]; //目标点云为最后(最新)的点云

            for (size_t i = 0; i < cloudsNum - 1; i++)
            {
                Eigen::Matrix4f rot;
                // std::cout << rot << std::endl;
                pclFunc.Gydata2RoT(Gys[i], Gys[cloudsNum - 1], rot);
                // Fusion(clouds[i], dest, rot);
                pclFunc.fusion(clouds[i], DepthImg, CL_RoT, camIntrix, rot);
            }
        }

        else //静态建图
        {
            std::thread Thread_saveImg(&Cam::getImg, &myCam);
            Thread_saveImg.detach();
            mylidar.getCloudImg(OriginCloud, DepthImg);
            myCam.getOriginImg(OriginImg);
            std::thread Thread_detect(&Cam::detectLag, &myCam, OriginImg);
            Thread_detect.detach();
            //从点云复原到深度图
            // Eigen::Vector4f CamCoor;
            // Eigen::Vector3f PxCoor;
            // for (size_t i = 0; i < OriginCloud->size(); i++)
            // {
            //     PointI tPoint = OriginCloud->points[i];
            //     float u, v;
            //     if (tPoint.x == 0 || tPoint.x > 10)
            //         continue;
            //     // if(abs(tPoint.y) > 6 || tPoint.z < -0.5 || tPoint.z > 1.7)
            //     //     continue;

            //     // LdCoor << tPoint.x, tPoint.y, tPoint.z, 1;
            //     CamCoor = CL_RoT * Eigen::Vector4f(tPoint.x, tPoint.y, tPoint.z, 1);
            //     PxCoor = camIntrix * Eigen::Vector3f(CamCoor.x(), CamCoor.y(), CamCoor.z());
            //     u = PxCoor(0) / CamCoor(2);
            //     v = PxCoor(1) / CamCoor(2);
            //     if (u < 0 || u > 1280 || v < 0 || v > 1024)
            //         continue;
            //     DepthImg.at<short>(v, u) = CamCoor(2) * 1000;
            // }
        }
        myCam.getLag(Lpoints, Tags);
        //画Lagori框
        copyOrigin = OriginImg.clone();
        for (size_t i = 0; i < Lpoints.size(); i++) //把全部识别到的Lagori在 OriginImg 框出来
        {
            cv::rectangle(OriginImg, cv::Rect(Lpoints[i].at(0), Lpoints[i].at(1)), cv::Scalar(122, 122, 122), 2);
            if (Tags[i] == 0)
            {
                cv::putText(OriginImg, "0", Lpoints[i].at(0), 1, 1, cv::Scalar(0, 0, 0));
            }
            else if (Tags[i] == 1)
            {
                cv::putText(OriginImg, "1", Lpoints[i].at(0), 1, 1, cv::Scalar(0, 0, 0));
            }
            else if (Tags[i] == 2)
            {
                cv::putText(OriginImg, "2", Lpoints[i].at(0), 1, 1, cv::Scalar(0, 0, 0));
            }
            else if (Tags[i] == 3)
            {
                cv::putText(OriginImg, "3", Lpoints[i].at(0), 1, 1, cv::Scalar(0, 0, 0));
            }
            else if (Tags[i] == 4)
            {
                cv::putText(OriginImg, "4", Lpoints[i].at(0), 1, 1, cv::Scalar(0, 0, 0));
            }
            else
            {
                cv::putText(OriginImg, "5", Lpoints[i].at(0), 1, 1, cv::Scalar(0, 0, 0));
            }
        }
        cv::imshow("OriginImg", OriginImg);
        cv::waitKey(1);
        // Lpoints.clear();
        // continue;

        // mylidar.getall(Lcloud, DepthImg);
        // while (Lcloud == nullptr || Lcloud->points.size() < pointsNum * 0.9)
        // {
        //     sleep(0.1);
        //     mylidar.getall(Lcloud, DepthImg);
        // }
        // cv::Mat weise;
        // cv::applyColorMap(DepthImg, weise, cv::COLORMAP_HOT);
        cv::imshow("DepthImg", DepthImg);
        cv::waitKey(1);

        pcl::visualization::CloudViewer V_cloud("Viewr OriginCloud");
        V_cloud.showCloud(OriginCloud);
        while (!V_cloud.wasStopped())
        {
        }

        //融合相机与雷达进行圆柱拟合
        for (size_t i = 0; i < Lpoints.size(); i++)
        {
            // std::cout << "第一个点的xy为: " << Lpoints[i][0].x << "  " << Lpoints[i][0].y << std::endl;
            // std::cout << "第二个点的xy为: " << Lpoints[i][1].x << "  " << Lpoints[i][1].y << std::endl;
            cv::rectangle(copyOrigin, cv::Rect(Lpoints[i][0], Lpoints[i][1]), cv::Scalar(180, 180, 180), 2); //在copyImg中每次框出来要圆柱拟合的Lagori
            cv::imshow("copy", copyOrigin);
            cv::waitKey(1);
            Pcloud tCloud(new Cloud());
            float Avx = 0, Avy = 0; //框出点的平均深度
            int id;
            bool color = Tags[i];
            int count = 0;
#if _OPENMP
#pragma omp parallel for num_threads(2)
#endif
            //进行点云反算，将框出来的Lagori重新显示到点云中，观察位置对应情况
            for (int col = Lpoints[i][0].x; col < Lpoints[i][1].x; col++)
            {
                for (int row = Lpoints[i][0].y; row < Lpoints[i][1].y; row++)
                {
                    Eigen::Vector3f CoorPixel; //像素坐标
                    Eigen::Vector3f CoorCam;   //相机坐标
                    Eigen::Vector4f CoorWorld; //世界坐标
                    float Depth = (float)DepthImg.at<int16_t>(row, col) / 1000.0;
                    if (Depth < 0.05) //小于50mm的点直接舍弃掉
                        continue;
                    // std::cout << Depth << ' ';
                    CoorPixel << Depth * col, Depth * row, Depth;
                    CoorCam = camIntrix.inverse() * CoorPixel;
                    CoorWorld = CL_RoT.inverse() * Eigen::Vector4f(CoorCam.x(), CoorCam.y(), CoorCam.z(), 1);
                    tCloud->points.push_back(Point(CoorWorld.x(), CoorWorld.y(), CoorWorld.z()));
                    Avx += CoorWorld.x();
                    Avy += CoorWorld.y();
                    count++;

                    PointI tPoint;
                    tPoint.x = CoorWorld.x();
                    tPoint.y = CoorWorld.y();
                    tPoint.z = CoorWorld.z();
                    tPoint.intensity = 1;
                    Lcloud->points.push_back(tPoint); //将lagori的所有点存下来
                }
                // std::cout << std::endl;
            }
            Avx = Avx / count;
            Avy = Avy / count;
            pcl::visualization::CloudViewer V_once("Viewer Every Lagori");
            V_once.showCloud(tCloud);
            while (!V_once.wasStopped())
            {
            }

            Pcloud OutCloud(new Cloud());
            Pcnormal OutNormals(new Cnormal());
            Pcnormal Normals(new Cnormal());
            pcl::ModelCoefficients::Ptr coe(new pcl::ModelCoefficients());
            pclFunc.computeNormals(tCloud, Normals, KsearchNum);
            Eigen::Vector2f CatchPoint;
            pclFunc.fitCylinder(tCloud, Normals, OutCloud, OutNormals, coe, CatchPoint, true);
            id = pclFunc.WhichId(coe->values[6] * 1000); //利用半径计算id
            CorLag tempLag;
            tempLag.id = id;
            tempLag.center.x() = Avx * 1000.;
            tempLag.center.y() = Avy * 1000.;
            std::cout << "平均值点为: " << tempLag.center.x() << "  " << tempLag.center.y() << std::endl;
            std::cout << "单点坐标为: " << CatchPoint.x() << "  " << CatchPoint.y() << std::endl;
            tempLag.radius = coe->values[6] * 1000.;
            tempLag.Lcolor = color;
            coordinateLags.push_back(tempLag); //将该Lagori的信息push_back

            if (OutCloud != nullptr)
            {
                std::cout << "fit succeed" << std::endl;
                pclFunc.axialDrawing(Lcloud, coe); //将拟合出的圆柱轴线画出来
                pcl::visualization::CloudViewer V_Lag("Viewer Lagori");
                V_Lag.showCloud(Lcloud);
                while (!V_Lag.wasStopped())
                {
                }
            }
        }
        //识别完成，发送所有Lagori信息
        LCMobj.Publish(coordinateLags);

        //  !!! --> 清除存储Lagori边框点、坐标点和点云的循环变量进行clear() <--- !!!
        Lpoints.clear();
        Lcloud->clear();
        coordinateLags.clear();
    }
}